#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(ros::roseus "people_with_socks_detector")

(ros::roseus-add-msgs "sensor_msgs")

(defvar *r-max* 255)
(defvar *r-min* 200)
(defvar *g-max* 50)
(defvar *g-min* 0)
(defvar *b-max* 50)
(defvar *b-min* 0)
(defvar *num-thr* 500)
(defvar *dis-thr* 500)

(defun pc-cb (msg)
  (let ((buf (make-3dpointcloud-from-msg2 msg)))
    (setq *3dp*
          (send buf :nfilter :ckey
                #'(lambda (x)
                    (and (< *r-min* *r-max*)
                         (< *g-min* *g-max*)
                         (< *b-min* *b-max*)))))))

(defun people-cb (msg)
  (let ((3dp (make-3dpointcloud-from-msg))
        (ret-lst (list)))
    (dolist (p (send 3dp :points))
      (when (< *num-thr*
               (send (send *3dp* :nfilter
                           :key #'(lambda (x) (< (distance x p) *dis-thr*))
                           :create t) :size))
        (push p ret-lst)))
    (let ((pub-msg (make-msg-from-3dpointcloud
                    (instance 3dpointcloud :init :points ret-lst))))
      (ros::publish "people_with_socks" pub-msg))))
  
(ros::subscribe "/camera/depth/points" sensor_msgs::PointCloud2 #'pc-cb)
(ros::subscribe "/people" sensor_msgs::PointCloud #'people-cb)
(ros::advertise "/people_with_socks" sensor_msgs::PointCloud 1)

(defun ros-proc ()
  (do-until-key
   (ros::spin-once)))

(ros-proc)